/*
 * @Author: Sian Li lisian_magic@163.com
 * @Date: 2024-07-01 16:01:59
 * @LastEditors: Sian Li lisian_magic@163.com
 * @LastEditTime: 2024-07-05 16:09:28
 * @FilePath: /hudar/src/hudar/src/RadarClass/Radar/include/Radar.h
 * @Description:
 */
#ifndef RADAR_H__
#define RADAR_H__
#include "../../Camera/Camera.h"
#include "../../Camera/VideoRecorder.h"
#include "../../Common/include/SharedQueue.h"
#include "../../Common/include/public.h"
#include "../../Detectors/include/ArmorDetector.h"
#include "../../Detectors/include/CarDetector.h"
#include "../../Detectors/include/depthProcesser.h"
#include "../../Location/include/MapMapping.h"
#include "../../Location/include/location.h"
#include "image_transport/image_transport.hpp"
#include "opencv2/opencv.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rm_msg/msg/location.hpp"
#include "rm_msg/msg/locations.hpp"

#include "sensor_msgs/msg/point_cloud2.hpp"
#include <chrono>
#include <cv_bridge/cv_bridge.h>

#include <pcl/common/common.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/image_encodings.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <std_msgs/msg/bool.hpp>
#include <std_msgs/msg/header.hpp>
#include <std_msgs/msg/string.hpp>
using namespace cv;

class Radar : public rclcpp::Node {

  public:
    // 构造函数,有一个参数为节点名称
    Radar(std::string name) : Node(name) {
        // 打印一句
        RCLCPP_INFO(this->get_logger(), "Radar Starting");
        this->init();
    }
    ~Radar();

  private:
    vector<vector<Point3f>> show_region =
        vector<vector<Point3f>>{{Point3f(15.682, 14.844 - 15.f, 0.3f),
                                 Point3f(23.464, 14.844 - 15.f, 0.3f),
                                 Point3f(23.464, 13.984 - 15.f, 0.3f),
                                 Point3f(15.682, 13.984 - 15.f, 0.3f)},
                                {Point3f(4.536, 1.016 - 15.f, 0.3f),
                                 Point3f(12.318, 1.016 - 15.f, 0.3f),
                                 Point3f(12.318, 0.156 - 15.f, 0.3f),
                                 Point3f(4.536, 0.156 - 15.f, 0.3f)}};

    const int ids[12] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11};
    // 参数
    // Yolo模型路径
    std::string _model_path;
    std::string mapping_file;
    std::string record_path;
    std::string _lidar_topic;
    std::string _camera_params;
    std::shared_ptr<HKCam> _camera;
    ArmorDetector::Ptr armorDetector;
    CarDetector::Ptr carDetector;
    DepthQueue::Ptr depthQueue;
    VideoRecorder::Ptr videoRecorder;
    Location::Ptr myLocation;
    MapMapping::Ptr mapMapping;
    // 线程
    bool radar_alive = false;
    bool _if_record = true;
    bool _recorder_block;
    bool _videoRecorderLoopWorking = false;
    bool _mainProcessLoopWorking = false;
    bool coverDepth;
    std::thread masterThread;
    std::thread mainLoop;
    std::thread videoRecoderLoop;
    cv::Mat K_0_Mat;
    cv::Mat C_0_Mat;
    cv::Mat E_0_Mat;
    std::vector<vector<float>> publicDepth;
    std::shared_timed_mutex myMutex_publicDepth;
    std::shared_timed_mutex myMutex_cameraThread;
    SharedQueue<Mat> myFrames;
    std::shared_ptr<spdlog::logger> logger = spdlog::get("RadarLogger");
    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_lidar;
    rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr sub_exit;
    rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr sub_cover;
    rclcpp::Publisher<rm_msg::msg::Locations>::SharedPtr pub_robo;
    rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr pub_image;
    bool _inited = false;
    void init();
    void stop();
    void LidarListenerBegin();
    void LidarCallBack(const sensor_msgs::msg::PointCloud2::SharedPtr msg);
    void VideoRecorderLoop();
    void mainProcessLoop();
    void armor_filter(vector<bboxAndRect> &pred);
    void detectDepth(vector<bboxAndRect> &pred);
    void detectDepth(vector<ArmorBoundingBox> &armors);
    void drawBbox(vector<DetectBox> &bboxs, Mat &img);
    void drawArmorsForDebug(vector<ArmorBoundingBox> &armors, Mat &img);
    void drawArmorsForDebug(vector<bboxAndRect> &armors, Mat &img);
    void coverDepthCallback(const std_msgs::msg::Bool::SharedPtr msg);
    void exitCallback(const std_msgs::msg::Bool::SharedPtr msg);
};

#endif